#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>

#include <opencv2/opencv.hpp>

#include "opencv2/features2d/features2d.hpp"
#include "opencv2/nonfree/nonfree.hpp"
#include "opencv2/legacy/legacy.hpp"

#include "mv_camera_tools.hpp"
#include "mv_pub.hpp"

using namespace std;

#define HGUI_DEBUG

int MvCameraTools::getCameraCalibrationPoint(Size imgSize, const float *curve, int curveSize, Point2f center,
		Point2f src, Point2f &dst) {

	int dstRow = imgSize.height;
	int dstCol = imgSize.width;

	int j2 = (src.y - dstRow / 2) * (src.y - dstRow / 2);
	float r = sqrt(j2 + (src.x - dstCol / 2) * (src.x - dstCol / 2));
	int ir = (int) r;
	if (ir >= curveSize - 1) {
		return -1;
	}

	float m = curve[ir] + (curve[ir + 1] - curve[ir]) * (r - ir);
	dst.x = center.x + (src.x - dstCol / 2) / m;
	dst.y = center.y + (src.y - dstRow / 2) / m;

	return 0;
}

// 计算源图座标校正后的座标
int MvCameraTools::getCameraCalibrationPoints(Size imgSize, const float *curve, int curveSize, Point2f center,
		InputArray srcPts, OutputArray dstPts) {

	int row = srcPts.getMat().size().height, col = srcPts.getMat().size().width;

	Point2f *data = (Point2f*) srcPts.getMat().data;

	Mat tmp(row, col, CV_32FC2, Scalar(0, 0));
	Point2f *dsts = (Point2f*) tmp.data;

	for (int j = 0; j < row; j++) {
		for (int i = 0; i < col; i++) {
			getCameraCalibrationPoint(imgSize, curve, curveSize, center, data[row * j + i], dsts[row * j + i]);
		}
	}

	tmp.copyTo(dstPts);

	return 0;
}

// Rs =  Rd * Curve[Rd];
// Rd = Rs * CurveA[Rr]
// 根据传入的变形曲线数据校正图片，输出校正后图片
int MvCameraTools::cameraCalibration(Mat &src, const float *curve, int curveSize, Point2f center, OutputArray dstImg) {

	int dstRow = src.size().height;
	int dstCol = src.size().width;

	Mat tmp(dstRow, dstCol, CV_32FC2, Scalar(0, 0));

	int srcStep = src.step;
	int srcRow = dstRow;
	int srcCol = dstCol;

	float p00, p01, p10, p11, pd;
	unsigned char* srcData = src.data;
	for (int j = 0; j < dstRow; j++) {
		unsigned char* dstData = tmp.ptr<unsigned char>(j);
		int j2 = (j - dstRow / 2) * (j - dstRow / 2);
		for (int i = 0; i < dstCol; i++) {
			float r = sqrt(j2 + (i - dstCol / 2) * (i - dstCol / 2));
			int ir = (int) r;
			if (ir >= curveSize) {
				dstData += 3;
				continue;
			}

			float m = curve[ir] + (curve[ir + 1] - curve[ir]) * (r - ir);
			Point2f xy(0.f, 0.f);
			xy.x = center.x + (i - dstCol / 2) * m;
			xy.y = center.y + (j - dstRow / 2) * m;
			if (xy.x >= srcCol - 1 || xy.y >= srcRow - 1 || xy.x < 0 || xy.y < 0) {
				dstData += 3;
				continue;
			}

			float x = xy.x;
			float y = xy.y;
			int ix = (int) xy.x;
			int iy = (int) xy.y;

			// 4个点合成1个点(座标换成４个点)
			float xd = x - ix, yd = y - iy;
			float xdn = 1 + ix - x, ydn = 1 + iy - y;
			int cp = iy * srcStep + ix * 3;

			p00 = srcData[cp] * xdn;
			p01 = srcData[cp + 3] * xd;
			pd = (p00 + p01) * ydn;
			p10 = srcData[cp + srcStep] * xdn;
			p11 = srcData[cp + srcStep + 3] * xd;
			pd += (p10 + p11) * yd;
			*dstData = pd; // 目标点数据R
			dstData++;

			cp++;
			p00 = srcData[cp] * xdn;
			p01 = srcData[cp + 3] * xd;
			pd = (p00 + p01) * ydn;
			p10 = srcData[cp + srcStep] * xdn;
			p11 = srcData[cp + srcStep + 3] * xd;
			pd += (p10 + p11) * yd;
			*dstData = pd; // 目标点数据G
			dstData++;

			cp++;
			p00 = srcData[cp] * xdn;
			p01 = srcData[cp + 3] * (xd);
			pd = (p00 + p01) * ydn;
			p10 = srcData[cp + srcStep] * xdn;
			p11 = srcData[cp + srcStep + 3] * xd;
			pd += (p10 + p11) * yd;
			*dstData = pd; // 目标点数据B
			dstData++;
		}
	}

	tmp.copyTo(dstImg);

	return 0;
}

// 根据传入的校正后的点座标计算校正前的点座标
int MvCameraTools::cameraInvCalibration(InputArray srcPts, const float *curve, int curveSize, Point2f center,
		OutputArray dstPts) {

	Mat matSrcPts = srcPts.getMat();
	int dstRow = matSrcPts.size().height;
	int dstCol = matSrcPts.size().width;

	Mat tmp(dstRow, dstCol, CV_32FC2, Scalar(0, 0));

	for (int j = 0; j < dstRow; j++) {
		Point2f *sl = matSrcPts.ptr<Point2f>(j);
		Point2f *dl = tmp.ptr<Point2f>(j);
		for (int i = 0; i < dstCol; i++) {
			float ry2 = (sl->y - dstRow / 2) * (sl->y - dstRow / 2);
			float rx2 = (sl->x - dstCol / 2) * (sl->x - dstCol / 2);
			float r = sqrt(ry2 + rx2); // 目标点半径

			int ir = (int) r;
			if (ir >= curveSize) {
				*dl = Point(0, 0);
				sl++;
				dl++;
				continue;
			}

			Point2f xy(0.f, 0.f);
			float m = curve[ir] + (curve[ir + 1] - curve[ir]) * (r - ir);
			xy.x = center.x + (sl->x - dstCol / 2) * m;
			xy.y = center.y + (sl->y - dstRow / 2) * m;

			if (xy.x >= dstCol - 1 || xy.y >= dstRow - 1 || xy.x < 0 || xy.y < 0) {
				*dl = Point(0, 0);
				sl++;
				dl++;
				continue;
			}

			*dl = xy;
			sl++;
			dl++;
		}
	}

	tmp.copyTo(dstPts);

	return 0;
}

